#include <constants.h>
#include "fnavfn/PlotTrajectory.h"
#include "fnavfn/Clear.h"

visualization_msgs::Marker point;
ros::Publisher marker_pub;

bool clear(fnavfn::Clear::Request  &req, fnavfn::Clear::Response &res ){
	point.points.clear();
	return true;
}

bool plot(fnavfn::PlotTrajectory::Request &req, fnavfn::PlotTrajectory::Response &res )
{
	int size = req.x.size();
	
	bool special = ( req.special != 0 )? true : false;
	geometry_msgs::Point p;
	
	if( special ){
		point.color.g = 0.0f;
		point.color.a = 1.0f;
	}
	
	for( int i=0; i<size; i++ ){
		p.x = req.x[i]; 
		p.y = req.y[i];
		p.z = 0;
		
		point.header.stamp = ros::Time::now();
		point.points.push_back(p);		
	}
	
	marker_pub.publish(point);
		
	res.sum = size;
	return true;
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "trajectory_plot_service");
	ros::NodeHandle n;
	ros::ServiceServer clear_service, plot_service;
	ros::Rate rate(10.0);
	
	/** Advertising services **/
	plot_service = n.advertiseService("trajectory_plot", plot);
	clear_service = n.advertiseService("clear_plots", clear);
	marker_pub = n.advertise<visualization_msgs::Marker>("trajectory_marker", 10);
	
	ROS_INFO("Ready to serve me lord");

	/** Init points **/
	point.header.frame_id = "/odom";
	
	point.ns = "trajectory_pts";
	point.action = visualization_msgs::Marker::ADD;
	point.pose.orientation.w = 1.0;

	point.id = 0;

	point.type = visualization_msgs::Marker::POINTS;

	// point markers use x and y scale for width/height respectively
	point.scale.x = 0.02f;
	point.scale.y = 0.02f;

	// point are yellow
	point.color.g = 1.0f;
	point.color.r = 1.0f;
	point.color.a = 1.0f;

	ros::spin();
	
	ROS_INFO("Allllllaaaah");

	return 0;
}
